#include "stdafx.h"
#include "Des.h"
#include <algorithm>
#include <intrin.h>

using namespace std;


static const byte pc1[] =
{
    56, 48, 40, 32, 24, 16, 8,
    0, 57, 49, 41, 33, 25, 17,
    9, 1, 58, 50, 42, 34, 26,
    18, 10, 2, 59, 51, 43, 35,
    62, 54, 46, 38, 30, 22, 14,
    6, 61, 53, 45, 37, 29, 21,
    13, 5, 60, 52, 44, 36, 28,
    20, 12, 4, 27, 19, 11, 3,
};

static const byte rotates[] =
{
    1, 2, 4, 6, 8, 10, 12, 14, 15, 17, 19, 21, 23, 25, 27, 28
};

static const byte pc2[] =
{
    13, 16, 10, 23, 0, 4,
    2, 27, 14, 5, 20, 9,
    22, 18, 11, 3, 25, 7,
    15, 6, 26, 19, 12, 1,
    40, 51, 30, 36, 46, 54,
    29, 39, 50, 44, 32, 47,
    43, 48, 38, 55, 33, 52,
    45, 41, 49, 35, 28, 31,
};

static const uint32 bigEndianByteBits[8] =
{
    0x80, 0x40, 0x20, 0x10, 0x08, 0x04, 0x02, 0x01
};

const uint32 spbox[8][64] =
{
    {
        0x01010400, 0x00000000, 0x00010000, 0x01010404,
        0x01010004, 0x00010404, 0x00000004, 0x00010000,
        0x00000400, 0x01010400, 0x01010404, 0x00000400,
        0x01000404, 0x01010004, 0x01000000, 0x00000004,
        0x00000404, 0x01000400, 0x01000400, 0x00010400,
        0x00010400, 0x01010000, 0x01010000, 0x01000404,
        0x00010004, 0x01000004, 0x01000004, 0x00010004,
        0x00000000, 0x00000404, 0x00010404, 0x01000000,
        0x00010000, 0x01010404, 0x00000004, 0x01010000,
        0x01010400, 0x01000000, 0x01000000, 0x00000400,
        0x01010004, 0x00010000, 0x00010400, 0x01000004,
        0x00000400, 0x00000004, 0x01000404, 0x00010404,
        0x01010404, 0x00010004, 0x01010000, 0x01000404,
        0x01000004, 0x00000404, 0x00010404, 0x01010400,
        0x00000404, 0x01000400, 0x01000400, 0x00000000,
        0x00010004, 0x00010400, 0x00000000, 0x01010004
    },
    {
        0x80108020, 0x80008000, 0x00008000, 0x00108020,
        0x00100000, 0x00000020, 0x80100020, 0x80008020,
        0x80000020, 0x80108020, 0x80108000, 0x80000000,
        0x80008000, 0x00100000, 0x00000020, 0x80100020,
        0x00108000, 0x00100020, 0x80008020, 0x00000000,
        0x80000000, 0x00008000, 0x00108020, 0x80100000,
        0x00100020, 0x80000020, 0x00000000, 0x00108000,
        0x00008020, 0x80108000, 0x80100000, 0x00008020,
        0x00000000, 0x00108020, 0x80100020, 0x00100000,
        0x80008020, 0x80100000, 0x80108000, 0x00008000,
        0x80100000, 0x80008000, 0x00000020, 0x80108020,
        0x00108020, 0x00000020, 0x00008000, 0x80000000,
        0x00008020, 0x80108000, 0x00100000, 0x80000020,
        0x00100020, 0x80008020, 0x80000020, 0x00100020,
        0x00108000, 0x00000000, 0x80008000, 0x00008020,
        0x80000000, 0x80100020, 0x80108020, 0x00108000
    },
    {
        0x00000208, 0x08020200, 0x00000000, 0x08020008,
        0x08000200, 0x00000000, 0x00020208, 0x08000200,
        0x00020008, 0x08000008, 0x08000008, 0x00020000,
        0x08020208, 0x00020008, 0x08020000, 0x00000208,
        0x08000000, 0x00000008, 0x08020200, 0x00000200,
        0x00020200, 0x08020000, 0x08020008, 0x00020208,
        0x08000208, 0x00020200, 0x00020000, 0x08000208,
        0x00000008, 0x08020208, 0x00000200, 0x08000000,
        0x08020200, 0x08000000, 0x00020008, 0x00000208,
        0x00020000, 0x08020200, 0x08000200, 0x00000000,
        0x00000200, 0x00020008, 0x08020208, 0x08000200,
        0x08000008, 0x00000200, 0x00000000, 0x08020008,
        0x08000208, 0x00020000, 0x08000000, 0x08020208,
        0x00000008, 0x00020208, 0x00020200, 0x08000008,
        0x08020000, 0x08000208, 0x00000208, 0x08020000,
        0x00020208, 0x00000008, 0x08020008, 0x00020200
    },
    {
        0x00802001, 0x00002081, 0x00002081, 0x00000080,
        0x00802080, 0x00800081, 0x00800001, 0x00002001,
        0x00000000, 0x00802000, 0x00802000, 0x00802081,
        0x00000081, 0x00000000, 0x00800080, 0x00800001,
        0x00000001, 0x00002000, 0x00800000, 0x00802001,
        0x00000080, 0x00800000, 0x00002001, 0x00002080,
        0x00800081, 0x00000001, 0x00002080, 0x00800080,
        0x00002000, 0x00802080, 0x00802081, 0x00000081,
        0x00800080, 0x00800001, 0x00802000, 0x00802081,
        0x00000081, 0x00000000, 0x00000000, 0x00802000,
        0x00002080, 0x00800080, 0x00800081, 0x00000001,
        0x00802001, 0x00002081, 0x00002081, 0x00000080,
        0x00802081, 0x00000081, 0x00000001, 0x00002000,
        0x00800001, 0x00002001, 0x00802080, 0x00800081,
        0x00002001, 0x00002080, 0x00800000, 0x00802001,
        0x00000080, 0x00800000, 0x00002000, 0x00802080
    },
    {
        0x00000100, 0x02080100, 0x02080000, 0x42000100,
        0x00080000, 0x00000100, 0x40000000, 0x02080000,
        0x40080100, 0x00080000, 0x02000100, 0x40080100,
        0x42000100, 0x42080000, 0x00080100, 0x40000000,
        0x02000000, 0x40080000, 0x40080000, 0x00000000,
        0x40000100, 0x42080100, 0x42080100, 0x02000100,
        0x42080000, 0x40000100, 0x00000000, 0x42000000,
        0x02080100, 0x02000000, 0x42000000, 0x00080100,
        0x00080000, 0x42000100, 0x00000100, 0x02000000,
        0x40000000, 0x02080000, 0x42000100, 0x40080100,
        0x02000100, 0x40000000, 0x42080000, 0x02080100,
        0x40080100, 0x00000100, 0x02000000, 0x42080000,
        0x42080100, 0x00080100, 0x42000000, 0x42080100,
        0x02080000, 0x00000000, 0x40080000, 0x42000000,
        0x00080100, 0x02000100, 0x40000100, 0x00080000,
        0x00000000, 0x40080000, 0x02080100, 0x40000100
    },
    {
        0x20000010, 0x20400000, 0x00004000, 0x20404010,
        0x20400000, 0x00000010, 0x20404010, 0x00400000,
        0x20004000, 0x00404010, 0x00400000, 0x20000010,
        0x00400010, 0x20004000, 0x20000000, 0x00004010,
        0x00000000, 0x00400010, 0x20004010, 0x00004000,
        0x00404000, 0x20004010, 0x00000010, 0x20400010,
        0x20400010, 0x00000000, 0x00404010, 0x20404000,
        0x00004010, 0x00404000, 0x20404000, 0x20000000,
        0x20004000, 0x00000010, 0x20400010, 0x00404000,
        0x20404010, 0x00400000, 0x00004010, 0x20000010,
        0x00400000, 0x20004000, 0x20000000, 0x00004010,
        0x20000010, 0x20404010, 0x00404000, 0x20400000,
        0x00404010, 0x20404000, 0x00000000, 0x20400010,
        0x00000010, 0x00004000, 0x20400000, 0x00404010,
        0x00004000, 0x00400010, 0x20004010, 0x00000000,
        0x20404000, 0x20000000, 0x00400010, 0x20004010
    },
    {
        0x00200000, 0x04200002, 0x04000802, 0x00000000,
        0x00000800, 0x04000802, 0x00200802, 0x04200800,
        0x04200802, 0x00200000, 0x00000000, 0x04000002,
        0x00000002, 0x04000000, 0x04200002, 0x00000802,
        0x04000800, 0x00200802, 0x00200002, 0x04000800,
        0x04000002, 0x04200000, 0x04200800, 0x00200002,
        0x04200000, 0x00000800, 0x00000802, 0x04200802,
        0x00200800, 0x00000002, 0x04000000, 0x00200800,
        0x04000000, 0x00200800, 0x00200000, 0x04000802,
        0x04000802, 0x04200002, 0x04200002, 0x00000002,
        0x00200002, 0x04000000, 0x04000800, 0x00200000,
        0x04200800, 0x00000802, 0x00200802, 0x04200800,
        0x00000802, 0x04000002, 0x04200802, 0x04200000,
        0x00200800, 0x00000000, 0x00000002, 0x04200802,
        0x00000000, 0x00200802, 0x04200000, 0x00000800,
        0x04000002, 0x04000800, 0x00000800, 0x00200002
    },
    {
        0x10001040, 0x00001000, 0x00040000, 0x10041040,
        0x10000000, 0x10001040, 0x00000040, 0x10000000,
        0x00040040, 0x10040000, 0x10041040, 0x00041000,
        0x10041000, 0x00041040, 0x00001000, 0x00000040,
        0x10040000, 0x10000040, 0x10001000, 0x00001040,
        0x00041000, 0x00040040, 0x10040040, 0x10041000,
        0x00001040, 0x00000000, 0x00000000, 0x10040040,
        0x10000040, 0x10001000, 0x00041040, 0x00040000,
        0x00041040, 0x00040000, 0x10041000, 0x00001000,
        0x00000040, 0x10040040, 0x00001000, 0x00041040,
        0x10001000, 0x00000040, 0x10000040, 0x10040000,
        0x10040040, 0x10000000, 0x00040000, 0x10001040,
        0x00000000, 0x10041040, 0x00040040, 0x10000040,
        0x10040000, 0x10001000, 0x10001040, 0x00000000,
        0x10041040, 0x00041000, 0x00041000, 0x00001040,
        0x00001040, 0x00040040, 0x10000000, 0x10041000
    }
};

inline int reverserOrder(uint32 v)
{
    return _byteswap_ulong(v);
}

inline int rol(uint32 v, uint8 n)
{
    return _rotl(v, n);
}

inline int ror(uint32 v, uint8 n)
{
    return _rotr(v, n);
}

namespace Algorithm
{
    Des::LRHalf getLR(const byte *inData)
    {
        const uint32 *p = reinterpret_cast<const uint32 *>(inData);
        int l = reverserOrder(*p++);
        int r = reverserOrder(*p);
        return Des::LRHalf { l, r };
    }

    void putLR(byte *outData, Des::LRHalf lr)
    {
        uint32 *p = reinterpret_cast<uint32 *>(outData);
        *p++ = reverserOrder(lr.r);
        *p = reverserOrder(lr.l);
    }

    Des::LRHalf ipExtand(Des::LRHalf lr)
    {
        int l = lr.l, r = lr.r;

        int temp = ((r = rol(r, 4)) ^ l) & 0xF0F0F0F0;
        l ^= temp, r ^= temp;
        temp = ((r = rol(r, 12)) ^ l) & 0xFFFF0000;
        l ^= temp, r ^= temp;
        temp = ((r = rol(r, 14)) ^ l) & 0x33333333;
        l ^= temp, r ^= temp;
        temp = ((r = ror(r, 6)) ^ l) & 0x00FF00FF;
        l ^= temp, r ^= temp;
        temp = ((r = rol(r, 9)) ^ l) & 0xAAAAAAAA;
        l ^= temp, r ^= temp;

        return Des::LRHalf { rol(l, 1), r };
    }

    inline Des::LRHalf fp(Des::LRHalf &lr)
    {
        int l = lr.l, r = lr.r;

        int temp = ((r = ror(r, 1)) ^ l) & 0xAAAAAAAA;
        l ^= temp, r ^= temp;
        temp = ((l = ror(l, 9)) ^ r) & 0x00FF00FF;
        l ^= temp, r ^= temp;
        temp = ((l = rol(l, 6)) ^ r) & 0x33333333;
        l ^= temp, r ^= temp;
        temp = ((l = ror(l, 14)) ^ r) & 0xFFFF0000;
        l ^= temp, r ^= temp;
        temp = ((l = ror(l, 12)) ^ r) & 0xF0F0F0F0;
        l ^= temp, r ^= temp;

        return Des::LRHalf { ror(l, 4), r };
    }

    void Des::setKey(const byte *key, Direction direction /*= Direction::Ecryption*/)
    {
        // pc1
        char pc1Applied[56];
        for (uint32 i = 0; i < 56; i++)
        {
            uint8 pc1Location = pc1[i];
            pc1Applied[i] = key[pc1Location >> 3] & bigEndianByteBits[pc1Location & 7] ? 1 : 0;
        }

        // 16 rounds
        for (uint32 round = 0; round < 16; round++)
        {
            // rotate
            char rotateApplied[56];
            for (uint32 i = 0; i < 56; i++)
            {
                uint8 limit = i < 28 ? 28 : 56;
                uint8 from = i + rotates[round];
                rotateApplied[i] = pc1Applied[from < limit ? from : from - 28];
            }

            // pc2
            char pc2Applied[48];
            for (uint32 i = 0; i < 48; i++)
            {
                pc2Applied[i] = rotateApplied[pc2[i]];
            }

            // ks
            uint32 ks[8] = { 0 }; // big-endian
            for (uint32 i = 0; i < 48; i++)
            {
                ks[i / 6] |= pc2Applied[i] ? bigEndianByteBits[i % 6] >> 2 : 0;
            }

            subkeys[round * 2] = ks[0]/*MSB*/ << 24
                | ks[2] << 16
                | ks[4] << 8
                | ks[6];
            subkeys[round * 2 + 1] = ks[1] << 24
                | ks[3] << 16
                | ks[5] << 8
                | ks[7] /*LSB*/;
        }

        if (direction != Direction::Ecryption)
        {
            for (uint32 i = 0; i < 16; i += 2)
            {
                swap(subkeys[i], subkeys[31 - 1 - i]);
                swap(subkeys[i + 1], subkeys[31 - i]);
            }
        }
    }

    Des::LRHalf Des::feistel(LRHalf &lr)
    {
        register uint32 l = lr.l, r = lr.r;

        // 2*8 = 16 rounds
        for (uint32 i = 0; i < 8; i++)
        {
            // 1st round
            uint32 temp = ror(r, 4) ^ subkeys[i * 4];
            l ^= spbox[6][(temp & 0x3F)]
                | spbox[4][((temp >> 8) & 0x3F)]
                | spbox[2][((temp >> 16) & 0x3F)]
                | spbox[0][((temp >> 24) & 0x3F)];
            temp = r ^ subkeys[i * 4 + 1];
            l ^= spbox[7][(temp & 0x3F)]
                | spbox[5][((temp >> 8) & 0x3F)]
                | spbox[3][((temp >> 16) & 0x3F)]
                | spbox[1][((temp >> 24) & 0x3F)];

            // 2nd round
            temp = ror(l, 4) ^ subkeys[i * 4 + 2];
            r ^= spbox[6][(temp & 0x3F)]
                | spbox[4][((temp >> 8) & 0x3F)]
                | spbox[2][((temp >> 16) & 0x3F)]
                | spbox[0][((temp >> 24) & 0x3F)];
            temp = l ^ subkeys[i * 4 + 3];
            r ^= spbox[7][(temp & 0x3F)]
                | spbox[5][((temp >> 8) & 0x3F)]
                | spbox[3][((temp >> 16) & 0x3F)]
                | spbox[1][((temp >> 24) & 0x3F)];
        }

        return LRHalf { l, r };
    }

    void Des::process(const byte *inData, byte *outData)
    {
        LRHalf lr = getLR(inData);
        lr = ipExtand(lr);
        lr = feistel(lr);
        lr = fp(lr);
        putLR(outData, lr);
    }
}